package util;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Timer;

/**
 *
 * @author TJ
 */
public class RateEncoder extends Encoder {
    double lastEncoderReading, lastTimeReading;

    public RateEncoder(int aSlot, int aChannel, int bSlot, int bChannel) {
        super(aSlot, aChannel, bSlot, bChannel);
        lastEncoderReading = 0.0;
        lastTimeReading = Timer.getFPGATimestamp();
    }

    public double pidGet() {
        double out = (getDistance() - lastEncoderReading) / (Timer.getFPGATimestamp() - lastTimeReading);
        lastEncoderReading = getDistance();
        lastTimeReading = Timer.getFPGATimestamp();
        return out;
    }
}
